#include "LOG.h"

void Log::init(){
	currentRec=0;
	for(int i=0; i<def_LOG_RECS_PER_PAGE; i++){
		workingPage.rec[i].init(0,0,0,0,0,0,0,0,0,0,0,0,0,0);
	}
	
}

void Log::addGPS(long lat, long lon, int confidence){
	
	workingPage.rec[currentRec].lat = lat;
	workingPage.rec[currentRec].lon = lon;
	workingPage.rec[currentRec].gpsTrust = confidence;
}


void Log::addIMU(int accelX, int accelY, int accelZ, int pitch, int roll, int yaw, int heading, int temp){
	workingPage.rec[currentRec].accelX = accelX;
	workingPage.rec[currentRec].accelY = accelY;
	workingPage.rec[currentRec].accelZ = accelZ;
	workingPage.rec[currentRec].pitch = pitch;
	workingPage.rec[currentRec].roll = roll;
	workingPage.rec[currentRec].yaw = yaw;
	workingPage.rec[currentRec].heading = heading;
	workingPage.rec[currentRec].temp = temp;
}

void Log::printPage(int pgNum){
#ifdef tst_PC
	LogPage pageOut = SpiPROM.readLogPage(pgNum*def_LOG_PAGE_LEN);
	Serial.println(pgNum);
	Serial.println();
	for(int i=0; i<def_LOG_RECS_PER_PAGE; i++){
		Serial.println(pageOut.rec[i].head);
			Serial.print(pageOut.rec[i].lat);
			Serial.print(" / "); 
			Serial.println(pageOut.rec[i].lon);
			Serial.println(pageOut.rec[i].gpsTrust);
			
			Serial.print(pageOut.rec[i].accelX);
			Serial.print(", ");
			Serial.print(pageOut.rec[i].accelY);
			Serial.print(", ");
			Serial.println(pageOut.rec[i].accelZ);

			Serial.print(pageOut.rec[i].pitch);
			Serial.print(", ");
			Serial.print(pageOut.rec[i].roll);
			Serial.print(", ");
			Serial.println(pageOut.rec[i].yaw);

			Serial.println(pageOut.rec[i].heading);

			Serial.println(pageOut.rec[i].temp);

			Serial.println(pageOut.rec[i].timeStamp);
			Serial.println(pageOut.rec[i].tail);
			Serial.println("END");
			Serial.println();
		}
			#endif
}

LogPage Log::BuffToPage(LogBuff inBuff){
	LogPage retPage;
	int bytePlace = 0;
			
	for(int c=0; c<def_LOG_RECS_PER_PAGE; c++){
		bytePlace = c*def_LOG_REC_LEN;
		for(int b=0; b<def_LOG_REC_LEN; b++){
			switch(b){
			    case 0:                                         //uint8_t head (0)
						retPage.rec[c].head = inBuff.data[(b+bytePlace)];
                        break;
                case 1:                                         //long lat (1-4)
                        retPage.rec[c].lat = *((long*)(&inBuff.data[(b+bytePlace)]));
                        break;
                case 5:                                         //long lon (5-8)
                        retPage.rec[c].lon = *((long*)(&inBuff.data[(b+bytePlace)]));
                        break;
                case 9:                                         //int gpsTrust (9-10)
                        retPage.rec[c].gpsTrust = *((int*)(&inBuff.data[(b+bytePlace)]));
                        break;
                case 11:                                                //int accelX (11-12)
                        retPage.rec[c].accelX = *((int*)(&inBuff.data[(b+bytePlace)]));   
                        break;
                case 13:                                                //int accelY (13-14)
                        retPage.rec[c].accelY = *((int*)(&inBuff.data[(b+bytePlace)]));      
                        break;
                case 15:                                                //int accelZ (15-16)
                        retPage.rec[c].accelZ = *((int*)(&inBuff.data[(b+bytePlace)]));     
                        break;
                case 17:                                                //int pitch (17-18)
                        retPage.rec[c].pitch = *((int*)(&inBuff.data[(b+bytePlace)]));       
                        break;
                case 19:                                                //int roll (19-20)
                        retPage.rec[c].roll = *((int*)(&inBuff.data[(b+bytePlace)]));
                        break;
                case 21:                                                //int yaw (21-22)
                        retPage.rec[c].yaw = *((int*)(&inBuff.data[(b+bytePlace)])); 
                        break;
                case 23:                                                //int heading (23-24)
                        retPage.rec[c].heading = *((int*)(&inBuff.data[(b+bytePlace)]));  
                        break;
                case 25:                                                //int temp (25-26)
                        retPage.rec[c].temp = *((int*)(&inBuff.data[(b+bytePlace)]));
                        break;
                case 27:                                                //unsigned long timeStamp (27-30)
                        retPage.rec[c].timeStamp = *((unsigned long*)(&inBuff.data[(b+bytePlace)]));
                        break;
                case 31:                                                //uint8_t tail (31)
                        retPage.rec[c].tail = inBuff.data[(b+bytePlace)];
                        break;
                default:
                        break;
			}
		}
	}
	return retPage;
}

LogBuff Log::PageToBuff(LogPage page){
	LogBuff retBuff;
	uint8_t* ptrByte;
	int bytePlace = 0;
	for(int c=0; c<def_LOG_RECS_PER_PAGE; c++){
		bytePlace = c*def_LOG_REC_LEN;
		for(int b=0; b<def_LOG_REC_LEN; b++){
			switch(b){
				case 0:						//uint8_t head (0)
					ptrByte = (uint8_t*)&(page.rec[c].head);
					break;
				case 1:						//long lat (1-4)
					ptrByte = (uint8_t*)&(page.rec[c].lat);
					break;
				case 5:						//long lon (5-8)
					ptrByte = (uint8_t*)&(page.rec[c].lon);
					break;
				case 9:						//int gpsTrust (9-10)
					ptrByte = (uint8_t*)&(page.rec[c].gpsTrust);
					break;
				case 11:						//int accelX (11-12)
					ptrByte = (uint8_t*)&(page.rec[c].accelX);	
					break;
				case 13:						//int accelY (13-14)
					ptrByte = (uint8_t*)&(page.rec[c].accelY);	
					break;
				case 15:						//int accelZ (15-16)
					ptrByte = (uint8_t*)&(page.rec[c].accelZ);	
					break;
				case 17:						//int pitch (17-18)
					ptrByte = (uint8_t*)&(page.rec[c].pitch);	
					break;
				case 19:						//int roll (19-20)
					ptrByte = (uint8_t*)&(page.rec[c].roll);	
					break;
				case 21:						//int yaw (21-22)
					ptrByte = (uint8_t*)&(page.rec[c].yaw);	
					break;
				case 23:						//int heading (23-24)
					ptrByte = (uint8_t*)&(page.rec[c].heading);	
					break;
				case 25:						//int temp (25-26)
					ptrByte = (uint8_t*)&(page.rec[c].temp);	
					break;
				case 27:						//unsigned long timeStamp (27-30)
					ptrByte = (uint8_t*)&(page.rec[c].timeStamp);	
					break;
				case 31:						//uint8_t tail (31)
					ptrByte = (uint8_t*)&(page.rec[c].tail);
					break;
				default:
					break;
			}
			retBuff.data[(b+bytePlace)] = *ptrByte;
			ptrByte++;
		}
	}

	return retBuff;
}


void LogRecord::init(uint8_t headParam, long latParam, long lonParam, int gpsTrustParam, int accelXParam, int accelYParam, int accelZParam, int pitchParam, int rollParam, int yawParam, int headingParam, int tempParam, unsigned long timeStampParam, uint8_t tailParam){
		head=headParam;
		lat=latParam;
		lon=lonParam;
		gpsTrust=gpsTrustParam;

		accelX=accelXParam;
		accelY=accelYParam;
		accelZ=accelZParam;
		pitch=pitchParam;
		roll=rollParam;
		yaw=yawParam;
		heading=headingParam;
		temp=tempParam;

		timeStamp=timeStampParam;
		tail=tailParam;
}
